#include "motorsTask.h"

#include "FreeRTOS.h"
#include "task.h"

//TODO: No deberia ser solo nxbot.h la unica libreria que habria que agregar?
#include "encoders.h"
#include <math.h>
static void motorsTask(void *pvParameters);
//int sum(int *buffer);

void executeMotorsTask(general *pvParameter)
{
	xTaskCreate(motorsTask,(const signed portCHAR * const) "motorTask",motorsTask_stackSize,(void *)pvParameter,tskIDLE_PRIORITY+2,( xTaskHandle * ) NULL);
}


//Tarea para solo actualizar las vels en la esctructura global
static void motorsTask(void *pvParameters)
{	
	//convertion of parameters 
	general *globals;
	globals=(general *)pvParameters;

	unsigned int freq;
	portTickType actualTick;

	//odometers direction
	unsigned char leftEncDir, rightEncDir;

	
	//variables used for odometry
	long change_left;
  long change_right;
  float transchange;
  float rotchange;
  float dx,dy;
  
	
	for(;;)
	{		

		//aquire the frequency for the task		
		
		freq=globals->gTimes.intervalMotors;
		
		//First update position
		globals->gEncod.posLeft = encoders_getStateLeft();
		globals->gEncod.posRight = encoders_getStateRight();
		
		//Update velocities
		motors_getActualVels(&(globals->gVels), MOTOR_INTERVAL_HZ);
		
		//Odometry calculation
		#if USE_ODOMETRY==1
		//calculate variations of position of wheels
		change_left = globals->gEncod.posLeft - globals->gEncod.lastPosLeft;
  	change_right = globals->gEncod.posRight - globals->gEncod.lastPosRight;
  
		//correction of encoders overflow
		if(change_left>60000)
			change_left = (globals->gEncod.posLeft-65535)-globals->gEncod.lastPosLeft;
	 	else if(change_left<-60000)
	 		change_left = (globals->gEncod.posLeft)+(65535-globals->gEncod.lastPosLeft);
	 	
	 	if(change_right>60000)
			change_right = (globals->gEncod.posRight-65535)-globals->gEncod.lastPosRight;
	 	else if(change_right<-60000)
	 		change_right = (globals->gEncod.posRight)+(65535-globals->gEncod.lastPosRight);
 	
	 	//update last positions
		globals->gEncod.lastPosLeft = globals->gEncod.posLeft;
		globals->gEncod.lastPosRight = globals->gEncod.posRight;

		transchange = (change_left + change_right) * NXBOT_ENCODER_RES / 2;
		rotchange = (change_left - change_right) * NXBOT_ENCODER_RES / NXBOT_WHEEL_DISTANCE;

		dx = transchange*cos(-globals->gRobotState.yaw - rotchange/2);
		dy = transchange*sin(-globals->gRobotState.yaw - rotchange/2);
		
		globals->gRobotState.posXAct += dx;
		globals->gRobotState.posYAct += dy;
		globals->gRobotState.yaw += rotchange;

		while (globals->gRobotState.yaw > 2*M_PI) 
			globals->gRobotState.yaw -= 2*M_PI;
		while (globals->gRobotState.yaw < 0) 
			globals->gRobotState.yaw += 2*M_PI;

		globals->gRobotState.orientation = globals->gRobotState.yaw;
		
		if(globals->gRobotState.orientation > M_PI)
  		globals->gRobotState.orientation -=2*M_PI;

  	globals->gRobotState.orientation *= -1;
		#endif
		
		//define direction of odometers, in order to increment or decrement them
		if(globals->gVels.velLeftRef>0)
			leftEncDir = ENC_FORWARD;
		else if(globals->gVels.velLeftRef<0)
			leftEncDir = ENC_REVERSE;
			
		if(globals->gVels.velRightRef>0)
			rightEncDir = ENC_FORWARD;
		else if (globals->gVels.velRightRef<0)
			rightEncDir = ENC_REVERSE;
		
		//Set direction in odometers
		if(globals->gVels.velLeftRef || globals->gVels.velRightRef)  
			encoders_setDirection(leftEncDir, rightEncDir);
		
		//then set velocity in motors
		motors_setVel(globals->gVels.velLeftRef,MOTOR_LEFT);
		motors_setVel(globals->gVels.velRightRef,MOTOR_RIGHT);
		
		//time delay according to the encodersInterval value
		actualTick = xTaskGetTickCount();
		vTaskDelayUntil( &actualTick,(portTickType)freq);		
	}
}

/*
int sum(int *buffer)
{
	int result;
	for(char i=0; i<8; i++)
		result += buffer[i];
	
	return result;
		
}
*/
